#include "CubicSpline2D.h"

CubicSpline2D::CubicSpline2D() = default;

CubicSpline2D::CubicSpline2D(const std::vector<std::pair<double, double>>& xy_points, 
                                                                const std::vector<double>& s, const int& index)
{
    std::vector<double> x, y;
    for(size_t i = 0; i < xy_points.size(); i++){
        x.push_back(xy_points[i].first);
        y.push_back(xy_points[i].second);
    }
    sx = CubicSpline1D(s, x);
    sy = CubicSpline1D(s, y);
}

SplineResult CubicSpline2D::calc_total(const double& s)
{
    double x = sx.calc_der0(s);
    double y = sy.calc_der0(s);

    double dx = sx.calc_der1(s);
    double dy = sy.calc_der1(s);
    double yaw = atan2(dy, dx);

    double ddx = sx.calc_der2(s);
    double ddy = sy.calc_der2(s);
    double kappa = (ddy * dx - ddx * dy) / pow(pow(dx, 2) + pow(dy, 2), 1.5);

    SplineResult result = {x, y, yaw, kappa};
    return result;
}
